#!/usr/bin/python3
import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, PushRosNamespace
from launch_ros.descriptions import ParameterFile

def generate_launch_description():
    # 定义机器类型
    robot_type = "trobot"

    # 配置目录、文件路径
    startup_dir = get_package_share_directory('trobot_startup')
    params_dir =  os.path.join(startup_dir, 'params')
    launch_dir =  os.path.join(startup_dir, 'launch')

    # 定义默认参数
    robot_type_arg = DeclareLaunchArgument(
        'robot_type', default_value='trobot_simu',
        description='Use simulation (Gazebo) clock if true')
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='false',
        description='Use simulation (Gazebo) clock if true')
    global_parameter_file_arg = DeclareLaunchArgument(
        'global_parameter_file', default_value = os.path.join(params_dir, 'global_params.yaml'),
        description='')
    default_robot_parameter_file_arg = DeclareLaunchArgument(
        'default_robot_parameter_file', default_value = os.path.join(params_dir, robot_type, 'default_robot_params.yaml'),
        description='')

    ld = LaunchDescription()
    # 启动硬件驱动launch
    hardware_controller_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(  
         get_package_share_directory('hardware_controller'), 'launch'),  
         '/hardware_controller.launch.py'])
    )
    # 启动激光雷达launch
    lslidar_driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(  
        #  get_package_share_directory('lslidar_driver'), 'launch'),  
        #  '/lslidar_launch.py'])
        get_package_share_directory('sllidar_ros2'), 'launch'),  
         '/sllidar_a1_launch.py'])
    )
    # 启动Imu
    wit_imu_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(  
         get_package_share_directory('wit_imu_ros2'), 'launch'),  
         '/wit_imu.launch.py'])
    )

    # 启动深度相机launch
    orbbec_camera_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(  
         get_package_share_directory('orbbec_camera'), 'launch'),  
         '/dabai_dcw.launch.py'])
    )

    # 启动IMU互补滤波

    # 启动激光雷达运动畸变处理
    lidar_2d_undistortion_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(  
            get_package_share_directory('sensors_preprocess'), 'launch'),  
            '/lidar_2d_undistortion.launch.py']),
        launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                }.items()
    )    

    laser_link_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        # arguments=['0.0', '0.0', '-0.09', '0.0', '0', '0', 'base_link', 'base_footprint']
        # 参数说明：x y z yaw pitch roll 父坐标系 子坐标系
        arguments=[
            '--x', '0.05', 
            '--y', '0.0', 
            '--z', '0.0',
            '--roll', '0.0',
            '--pitch', '0.0',
            '--yaw', '0.0',
            '--frame-id', 'base_link',
            '--child-frame-id', 'laser_link'
        ]
    )

    imu_link_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        # arguments=['0.0', '0.0', '-0.09', '0.0', '0', '0', 'base_link', 'base_footprint']
        # 参数说明：x y z yaw pitch roll 父坐标系 子坐标系
        arguments=[
            '--x', '0.0', 
            '--y', '0.0', 
            '--z', '0.0',
            '--roll', '0.0',
            '--pitch', '0.0',
            '--yaw', '0.0',
            '--frame-id', 'base_link',
            '--child-frame-id', 'imu_link'
        ]
    )

    camera_link_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        # arguments=['0.0', '0.0', '-0.09', '0.0', '0', '0', 'base_link', 'base_footprint']
        # 参数说明：x y z yaw pitch roll 父坐标系 子坐标系
        arguments=[
            '--x', '0.13', 
            '--y', '0.0', 
            '--z', '0.23',
            '--roll', '0.0',
            '--pitch', '0.0',
            '--yaw', '0.0',
            '--frame-id', 'base_link',
            '--child-frame-id', 'camera_link'
        ]
    )

    base_footprint_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        # arguments=['0.0', '0.0', '-0.09', '0.0', '0', '0', 'base_link', 'base_footprint']
        # 参数说明：x y z yaw pitch roll 父坐标系 子坐标系
        arguments=[
            '--x', '0.13', 
            '--y', '0.0', 
            '--z', '0.23',
            '--roll', '0.0',
            '--pitch', '0.0',
            '--yaw', '0.0',
            '--frame-id', 'base_link',
            '--child-frame-id', 'base_footprint'
        ]
    )


    return LaunchDescription(
        [
            global_parameter_file_arg,
            default_robot_parameter_file_arg,
            hardware_controller_launch,
            lslidar_driver_launch,
            wit_imu_launch,
            orbbec_camera_launch,
            lidar_2d_undistortion_launch,
            laser_link_tf,
            imu_link_tf,
            camera_link_tf,
            base_footprint_tf
        ]
    )